
%% Saturn
warning off
clc;clear;
format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-13,'AbsTol',1e-13);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 5000, ...
    'MaxIterations', 5000,'ConstraintTolerance',1e-6,'Algorithm','Interior-point','stepTolerance',1e-21);
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.


planetNumb = 1;
[L1,L2,L3,L4,L5] = librationPoints(u(planetNumb));
L_ = [L1,L2,L3,L4,L5];
for ii = 1:length(L_)
    L_pos = L_(:,ii);
    J_L(ii) = jacobiConst(L_pos,zeros(3,1),u(planetNumb));
end

%% environment settings
dt = 0.01;
startphase=0; % changing the starting position of the periodic
% orbit to show changes in the pole visibility based on the Satellite's position wrt the Sun's pole
% OpenCRTBP_u(u)
%% Define inertial frame FNs
[xs,ys,zs] = ellipsoid(-0,0,0,696340,696340,696340,180);
figure;
s = surface(xs,ys,zs);
rotate(s, [1 0 0], 7.25);
rotate(s, [0 0 1], 73.67+0.014*(2023-1850));
InclinationRotationAngle = 7.25;
RAANRotationAngle = 73.67+0.014*(2023-1850);
PoleDirection = Rotation([0;0;1],RAANRotationAngle,'Degrees')*...
    Rotation([1;0;0],InclinationRotationAngle,'Degrees')*...
    [0;0;1];
rotate(s, PoleDirection, -70);

FN_i = s.VertexNormals;
%%
load('SE_L4_Vertical.mat');
inc_list = zeros(1,length(T_L4_VL));
for ii = 1:length(T_L4_VL)
    IC = IC_L4_VL(:,ii);
    T = T_L4_VL(ii);
    [t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:dt:T],IC,options);
    S = S';

    S_i = zeros(size(S));
    S_i = C2I_primary(S(:,ii),u,DU,VU,t(ii)+5.1+startphase);
    COE_list = State2Coe(S_i,GM_central);
    inc_list(ii) = COE_list(3);
end

%% Simulation paramters %%
CameraAngleCapability=2;
inc_desired = 14.5;
optval = min(abs(inc_list-inc_desired));
[ind_opt] = find(abs(inc_list-inc_desired)==optval);

%%
IC_L4 = IC_L4_VL(:,ind_opt);
T_L4 = 2*pi;
IC_L5 = [IC_L4(1);-IC_L4(2);0;...
    -IC_L4(4);IC_L4(5);IC_L4(6)];

OpenCRTBP_u(u); hold on
% L4= DetermineOptimalL4andL5State(IC_L4,T_L4,u,DU,VU,TU,dt);
% L5= DetermineOptimalL4andL5State(IC_L5,T_L4,u,DU,VU,TU,dt);
[row,col,~] = size(FN_i);
% LatitudeInfo_L5=zeros(1,length(L4.epoch));

alpha_list = [0,90,180,270];

for oo = 1:length(alpha_list)
    L4= DetermineOptimalL4andL5State_varyingAlpha(IC_L4,T_L4,u,DU,VU,TU,dt,alpha_list(oo));
    
    for k = 1:length(L4.S_i)
        posvec = L4.S_i(1:3,k);
        checker = 0;
        for ii = 1:col
            for jj = 1:row
                normvec = [FN_i(ii,jj,1);FN_i(ii,jj,2);FN_i(ii,jj,3)];
                angle_temp = acosd(dot(normvec,posvec)/(norm(normvec)*norm(posvec)));
                if angle_temp < CameraAngleCapability
                    LatitudeInfo_L4{oo}(k) = ii-90;
                    checker=1;
                    break
                end
            end

            if checker == 1
                break
            end
            
        end
    end
end

L4.epoch = datetime(L4.epoch, 'Locale', 'fi_FI');
linestyle = {'-','--','-.',':'};
% linemarker={'s','d','h','o'}
linecolors={'k','r','g','b'};

figure;hold on;
for ii = 1:length(alpha_list)
    plot(L4.epoch,LatitudeInfo_L4{ii},"DatetimeTickFormat","yyyy",'Color',linecolors{ii},'linewidth',1);
end
legend('$\alpha$ = 0','$\alpha$ = 90','$\alpha$ = 180','$\alpha$ = 270','Interpreter','latex')
xlabel('Time [Yr]','Interpreter','latex')
ylabel('Heliographic Latitude[deg] ($\Theta = 0$)','Interpreter','latex')
xlim([datetime("2023-01-01") datetime("2026-01-01")])
ylim([-23,23])
set(gca,'fontsize',12)